/*******************************************************************************
 * Version: 1.0.0
 *
 * \file   main.c
 * \brief  This application demonstrate basing functionality
 *
 *******************************************************************************
 * <b> (C) Copyright 2016 Silicon Labs, http://www.silabs.com </b>
 *******************************************************************************
 * This file is licensed under the Silicon Labs License Agreement. See the file
 * "Silabs_License_Agreement.txt" for details. Before using this software for
 * any purpose, you must agree to the terms of that agreement.
 *
 ******************************************************************************/

/*******************************************************************************
 *******************************   INCLUDES   **********************************
 ******************************************************************************/
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include "em_device.h"
#include "native_gecko.h"
#include "gecko_configuration.h"
#include "gatt_db.h"
#include "bg_types.h"

#include "InitDevice.h"
#include "bg_dfu.h"
#include "aat.h"

#include "sleep.h"

#include "em_gpio.h"
#include "em_usart.h"
#include "em_int.h"
#include "dmadrv.h"

#include "misc.h"

/*******************************************************************************
 *****************************   LOCAL DATA   **********************************
 ******************************************************************************/

uint8_t bluetooth_stack_heap[DEFAULT_BLUETOOTH_HEAP(MAX_CONNECTIONS)];

/* Gecko configuration parameters (see gecko_configuration.h) */
static const gecko_configuration_t config = 
{
    .config_flags=0,

    .sleep.flags=SLEEP_FLAGS_ENABLE_WAKEUP_PIN,//SLEEP_FLAGS_ENABLE_WAKEUP_PIN,//SLEEP_FLAGS_DEEP_SLEEP_ENABLE | SLEEP_FLAGS_ACTIVE_HIGH | 
    .sleep.port=2,//PC
    .sleep.pin=11,//pin11

    .bluetooth.max_connections=MAX_CONNECTIONS,
    //.bluetooth.max_advertisers=MAX_ADVERTISERS,
    .bluetooth.heap=bluetooth_stack_heap,
    .bluetooth.heap_size=sizeof(bluetooth_stack_heap),
    .bluetooth.sleep_clock_accuracy = 100, // ppm

    .gattdb=&bg_gattdb_data,

#if (EFR32BG1_BRD4100A)
    .pa.config_enable = true,
    .pa.input = GECKO_RADIO_PA_INPUT_DCDC,
#else
    .pa.config_enable = true,
    .pa.input = GECKO_RADIO_PA_INPUT_VBAT,
#endif
    .ota.flags=0,
    .ota.device_name_len=3,
    .ota.device_name_ptr="OTA",

    .pti=NULL,
};


BleCB blecb = {
    .local_mac.addr = {0},
#if (EFR32BG1_BRD4100A)
    .txpower = 0,
#elif (EFR32BG1B232F256GM48)
    .txpower = 100,
#elif (EFR32BG1P332F256GM32)
    .txpower = 190,
#endif
    .pin = 1234,
    .appsta = STOP,
    .advsta = STOP,
    .scansta = STOP,
    .workstyle = CLIENT,
    .scanfilter = {0},
    .scanfilterlen = 0,
    .atcmdbuff = {0},
    .connindex = 0,
    .usart0dmachannel = 0,
    .usart0dmarxfifo = {0},
    .connections = 0,

    .conncb[0].mac.addr = {0},
    .conncb[0].connectedid = 0xFF,
    .conncb[0].mainservice = 1,
    .conncb[0].discoverserviceindex = 0,
    .conncb[0].service_uuid = {"\x00\x18","\x0D\x18","\x0F\x18"},
    .conncb[0].service_uuid_bit = {16,16,16}, 
    .conncb[0].charact_uuid = {{"\x00\x2A",0},{"\x37\x2A",0},{"\x19\x2A",0}},
    .conncb[0].charact_uuid_bit = {{16,0},{16,0},{16,0}}, 
    .conncb[0].service_handle = {0},
    .conncb[0].charact_handle = {0},
    .conncb[0].charact_properties = {0},
    .conncb[0].discoverstate = SCANNING,
    .conncb[0].bondsta = DISABLE,
    .conncb[0].pin = 1234,
    .conncb[0].discontime = 0,
#if (MAX_CONNECTIONS > 1)
    .conncb[1] = CONNCB_DEFAULT,
#endif
};


bool USART0_DMA_Callback(unsigned int channel,unsigned int sequenceNo,void *userParam);
void USART0_DMA_Init(void);
void USART0_SendStr(char *str);
void USART0_SendInt32(uint32_t d);
void USART0_SendHex(uint8_t d);
void USART0_SendBuffer(uint8_t data[],uint8_t len);

void print_mac(bd_addr addr);
uint8_t chenge_work_style(uint8_t style);
uint8_t replyATCMD(char *atcmd);
void resp_evt_scan_response(struct gecko_cmd_packet* evt);

/*******************************************************************************
 * @brief
 *   Main()
 * @param
 *   None
 * @return
 *   None
 ******************************************************************************/
void main(void)
{
    // Initialize Blue Gecko module
    enter_DefaultMode_from_RESET();
    gecko_init(&config);
    USART0_DMA_Init();

    while (1) 
    {
        /* Event pointer for handling events */
        struct gecko_cmd_packet* evt;
        uint16_t result = 0,i = 0;
        /* Check for stack event. */
        evt = gecko_wait_event();

        /* Handle events */
        switch (BGLIB_MSG_ID(evt->header)) 
        {
            /* This boot event is generated when the system boots up after reset. */
            case gecko_evt_system_boot_id:
                txpSetTransmitPower(blecb.txpower);
                cpy2Array(blecb.local_mac.addr,gecko_cmd_system_get_bt_address()->address.addr,6);

                if(blecb.workstyle == CLIENT)
                {
#if (BONDABLE)
                    gecko_cmd_sm_set_bondable_mode(1);
                    gecko_cmd_sm_configure(0x08,sm_io_capability_displayonly);
                    gecko_cmd_sm_set_passkey(blecb.conncb[blecb.connindex].pin);
#else
                    gecko_cmd_sm_set_bondable_mode(0);
#endif
                    //gecko_cmd_le_gap_set_adv_parameters(800,800,7);//500ms
                    //gecko_cmd_le_gap_set_mode(le_gap_general_discoverable,le_gap_undirected_connectable);
                    //gecko_cmd_le_gap_set_conn_parameters(0x06,0x06,0x00,0x0A);

                    blecb.connindex = findOneEmptyIndex(blecb);
                    gecko_cmd_le_gap_discover(le_gap_discover_observation);               
                    gecko_cmd_gatt_set_max_mtu(MTU_LEN + 3);

                    gecko_cmd_gatt_server_write_attribute_value(gattdb_firmware_version,0,sizeof(PRG_VERSION) - 2,PRG_VERSION);
                } 
#if (EFR32BG1_BRD4100A)
                gecko_cmd_hardware_set_soft_timer(32768/2,TIMER_LED,0);
#endif
                USART0_SendHex(RETURN_MSG_SYS_BOOT);
                USART0_SendStr(":system boot\r\n");
            break;

            case gecko_evt_le_gap_scan_response_id:
                if(blecb.workstyle == CLIENT)
                {
                    resp_evt_scan_response(evt);

                    //bd_addr wahiltmac = {.addr = {0xf0,0x5f,0xa0,0x09,0xb3,0xcd}};
                    //if(!cmp2Array(wahiltmac.addr,evt->data.evt_le_gap_scan_response.address.addr,6))
                    //    break;
                    //USART0_SendStr("Scan...\r\n");
                    //USART0_SendStr("Rssi: ");USART0_SendHex(evt->data.evt_le_gap_scan_response.rssi);USART0_SendStr("\r\n");
                    //USART0_SendStr("Packet type: ");USART0_SendHex(evt->data.evt_le_gap_scan_response.packet_type);USART0_SendStr("\r\n");
                    //USART0_SendStr("Address: ");print_mac(evt->data.evt_le_gap_scan_response.address);USART0_SendStr("\r\n");
                    //USART0_SendStr("Address type: ");USART0_SendHex(evt->data.evt_le_gap_scan_response.address_type);USART0_SendStr("\r\n");
                    //USART0_SendStr("Bonding: ");USART0_SendHex(evt->data.evt_le_gap_scan_response.bonding);USART0_SendStr("\r\n");
                    //USART0_SendStr("Data: ");
                    //for(i = 0;i < evt->data.evt_le_gap_scan_response.data.len;i ++)
                    //{
                    //    USART0_SendHex(evt->data.evt_le_gap_scan_response.data.data[i]);USART0_SendStr(" ");
                    //}
                    //USART0_SendStr("\r\n\r\n");
                }
            break;

            case gecko_evt_le_connection_bt5_opened_id:
            case gecko_evt_le_connection_opened_id:
                blecb.connections ++;
                if(blecb.connections > MAX_CONNECTIONS)
                {
                    blecb.connections = MAX_CONNECTIONS;
                    gecko_cmd_le_gap_end_procedure();
                    USART0_SendHex(RETURN_MSG_ATCMD_SCANSTOP);
                    USART0_SendStr(":SCAN STOP\r\n");
                    break;
                }

                if(evt->data.evt_le_connection_opened.master)
				{
                    if(blecb.connindex >= MAX_CONNECTIONS)   break;

					blecb.conncb[blecb.connindex].connectedid = evt->data.evt_le_connection_opened.connection;
                    cpy2Array(blecb.conncb[blecb.connindex].mac.addr,evt->data.evt_le_connection_opened.address.addr,6);

                    //if(blecb.workstyle == CLIENT)
                    //{
                        //gecko_cmd_gatt_discover_primary_services(evt->data.evt_le_connection_opened.connection);
                        //blecb.conncb[blecb.connindex].discoverstate = FIND_SERVICE;
                    //}
				}

                USART0_SendHex(RETURN_MSG_OPENED4);
                USART0_SendStr(":OPENED MAC: ");
                print_mac(blecb.conncb[blecb.connindex].mac);
                USART0_SendStr("\r\n");
            break;

            case gecko_evt_le_connection_closed_id:
                blecb.connections --;
                blecb.connindex = findConnectHandleIndex(blecb,evt->data.evt_le_connection_closed.connection);
                if(blecb.connindex == 0xff)
                {
                    //gecko_cmd_system_reset(0);
                    break;
                }

                if(blecb.workstyle == CLIENT)
                {
                    blecb.conncb[blecb.connindex].connectedid = 0xFF;
                    memset(blecb.conncb[blecb.connindex].mac.addr,0,6);
                    blecb.conncb[blecb.connindex].discoverserviceindex = 0;
                    blecb.conncb[blecb.connindex].discoverstate = SCANNING;
                    memset((uint8_t *)blecb.conncb[blecb.connindex].service_handle,0,sizeof(blecb.conncb[blecb.connindex].service_handle));
                    memset((uint8_t *)blecb.conncb[blecb.connindex].charact_handle,0,sizeof(blecb.conncb[blecb.connindex].charact_handle));
                    blecb.conncb[blecb.connindex].bondsta = DISABLE;
                    blecb.conncb[blecb.connindex].discontime = 0;

                    gecko_cmd_le_gap_discover(le_gap_discover_observation);               
                    USART0_SendHex(RETURN_MSG_ATCMD_SCANSTART);
                    USART0_SendStr(":SCAN START\r\n");
                }
                USART0_SendHex(RETURN_MSG_CLOSED);
                USART0_SendStr(":CLOSED\r\n");
            break;

            case gecko_evt_gatt_server_characteristic_status_id:
                if((evt->data.evt_gatt_server_characteristic_status.characteristic == gattdb_spp_data) &&  
                        (evt->data.evt_gatt_server_characteristic_status.status_flags == gatt_server_client_config)) 
                {
                    if(evt->data.evt_gatt_server_characteristic_status.client_config_flags == gatt_notification)
                    {
                        //charsta = ENABLE;
                        USART0_SendHex(RETURN_MSG_NOTIFY_EN);
                        USART0_SendStr(":NOTIFY ENABLE\r\n");
                    }
                    else if(evt->data.evt_gatt_server_characteristic_status.client_config_flags == gatt_disable)
                    {
                        //charsta = DISABLE;
                        USART0_SendHex(RETURN_MSG_NOTIFY_DIS);
                        USART0_SendStr(":NOTIFY DISABLE\r\n");
                    }
                }
            break;

            case gecko_evt_gatt_server_attribute_value_id:
                //if((evt->data.evt_gatt_server_attribute_value.connection == conConnectionId) && (evt->data.evt_gatt_server_attribute_value.attribute == gattdb_spp_data))
                //{
                //    USART0_SendBuffer(evt->data.evt_gatt_server_attribute_value.value.data,evt->data.evt_gatt_server_attribute_value.value.len);
                //}
            break;

            case gecko_evt_hardware_soft_timer_id:
                switch (evt->data.evt_hardware_soft_timer.handle) 
                {
                    case TIMER_RERAD_CHARACT:
                    {
                        static uint8_t conn = 0,m = 0,n = 0;
                        uint8_t q = 0;
                        while(1)
                        {
                            if(blecb.conncb[conn].charact_handle[m][n] && (blecb.conncb[conn].charact_properties[m][n] & ePROPERTIES_Read))
                            {
                                gecko_cmd_gatt_read_characteristic_value(blecb.conncb[conn].connectedid,blecb.conncb[conn].charact_handle[m][n]);
                                q = 1;
                            }
                            if(++ n > MAX_EVERY_SERVICE_CHARACT_UUID_COUNTS)
                            {
                                n = 0;
                                if(++ m > MAX_SERVICE_UUID_COUNTS)
                                {
                                    m = 0;
                                    if(++ conn > MAX_CONNECTIONS)
                                    {
                                        conn = 0;
                                    }
                                    break;
                                }
                                if(q)
                                {
                                    q = 0;
                                    break;
                                }
                            }
                        }
                    }
                    break;
#if (EFR32BG1_BRD4100A)
                    case TIMER_LED:

                        GPIO_PinOutToggle(gpioPortF, 4);
                    break;
#endif
                    default:
                    break;
                }
            break;

            case gecko_evt_system_external_signal_id:
                switch(evt->data.evt_system_external_signal.extsignals)
                {
                    case EXSG_WAKEUP:
                        {
                            if(sleepEM2 == SLEEP_LowestEnergyModeGet())
                            {
                                SLEEP_SleepBlockEnd(sleepEM3); //END EM2
                                SLEEP_SleepBlockBegin(sleepEM2);//GO EM1
                            }
                        }
                    break;
                    case EXSG_USART0_RX_DMA_DONE:
                    case EXSG_USART0_RX_DMA_TIMEOUT:
                        {
                            USART0_DMA_Callback(blecb.usart0dmachannel,0,0);
                        }
                    break;
                    case EXSG_RECV_USR_AT_CMD:
                        {
                            replyATCMD(blecb.atcmdbuff);
                        }
                    break;

                    default:
                    break;
                }
            break;

#if (BONDABLE)
            case gecko_evt_sm_confirm_bonding_id:
                gecko_cmd_sm_bonding_confirm(evt->data.evt_sm_confirm_bonding.connection,1);
            break;

            case gecko_evt_sm_bonded_id:
                blecb.conncb[blecb.connindex].bondsta = ENABLE; 
                USART0_SendHex(RETURN_MSG_BONDED);
                USART0_SendStr(":BONDED\r\n");
            break;
#endif

            case gecko_evt_gatt_service_id:
                if(blecb.conncb[blecb.connindex].connectedid != evt->data.evt_gatt_service.connection)
                {
                    gecko_cmd_endpoint_close(evt->data.evt_gatt_service.connection);
                    break;
                }
                for(i = 0;i < MAX_SERVICE_UUID_COUNTS;i ++)
                {
                    if( cmp2Array(evt->data.evt_gatt_service.uuid.data,
                        (uint8_t *)blecb.conncb[blecb.connindex].service_uuid[i],
                        evt->data.evt_gatt_service.uuid.len))
                    {
                        blecb.conncb[blecb.connindex].service_handle[i] = evt->data.evt_gatt_service.service;
                        break;
                    }
                }
                blecb.conncb[blecb.connindex].discoverserviceindex = 0;

                USART0_SendStr("00:Service discover,handel: ");
                USART0_SendInt32(blecb.conncb[blecb.connindex].service_handle[i]);
                USART0_SendStr(" uuid: 0x");
                for(i = evt->data.evt_gatt_service.uuid.len;i > 0 ;i --)
                {
                    USART0_SendHex(evt->data.evt_gatt_service.uuid.data[i - 1]);
                }
                USART0_SendStr("\r\n");
            break;

            case gecko_evt_gatt_characteristic_id:
                if(blecb.conncb[blecb.connindex].connectedid != evt->data.evt_gatt_characteristic.connection)
                {
                    gecko_cmd_endpoint_close(evt->data.evt_gatt_characteristic.connection);
                    break;
                }
                for(i = 0;i < MAX_EVERY_SERVICE_CHARACT_UUID_COUNTS;i ++)
                {
                    if( cmp2Array(evt->data.evt_gatt_characteristic.uuid.data,
                        (uint8_t *)blecb.conncb[blecb.connindex].charact_uuid[blecb.conncb[blecb.connindex].discoverserviceindex][i],
                        evt->data.evt_gatt_characteristic.uuid.len))
                    {
                        blecb.conncb[blecb.connindex].charact_handle[blecb.conncb[blecb.connindex].discoverserviceindex][i] = 
                            evt->data.evt_gatt_characteristic.characteristic;
                        blecb.conncb[blecb.connindex].charact_properties[blecb.conncb[blecb.connindex].discoverserviceindex][i] = 
                            evt->data.evt_gatt_characteristic.properties;
                        break;
                    }
                }

                USART0_SendStr("00:Character discover,handel: ");
                USART0_SendInt32(blecb.conncb[blecb.connindex].charact_handle[blecb.conncb[blecb.connindex].discoverserviceindex][i]);
                USART0_SendStr(" properties: 0x");
                USART0_SendHex(blecb.conncb[blecb.connindex].charact_properties[blecb.conncb[blecb.connindex].discoverserviceindex][i]);
                USART0_SendStr(" uuid: 0x");
                for(i = evt->data.evt_gatt_characteristic.uuid.len;i > 0;i --)
                {
                    USART0_SendHex(evt->data.evt_gatt_characteristic.uuid.data[i - 1]);
                }
                USART0_SendStr("\r\n");

            break;

            case gecko_evt_gatt_characteristic_value_id:
                //hr service charact frmat -> https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.characteristic.heart_rate_measurement.xml
                //USART0_SendBuffer(evt->data.evt_gatt_characteristic_value.value.data,evt->data.evt_gatt_characteristic_value.value.len);
                i = findConnectHandleIndex(blecb,evt->data.evt_gatt_characteristic_value.connection);
                uint8_t m = 0,n = 0;
                for(m = 0;m < MAX_SERVICE_UUID_COUNTS;m ++)
                {
                    for(n = 0;n < MAX_EVERY_SERVICE_CHARACT_UUID_COUNTS;n ++)
                    {
                        if(evt->data.evt_gatt_characteristic_value.characteristic == blecb.conncb[i].charact_handle[m][n])
                        {
                            if(evt->data.evt_gatt_characteristic_value.att_opcode == gatt_handle_value_notification)
                            {
                                USART0_SendStr("Ntfy:");
                            }
                            else if(evt->data.evt_gatt_characteristic_value.att_opcode == gatt_read_response)
                            {
                                USART0_SendStr("Read:");
                            }
							else
							{
								USART0_SendStr("None:");
							}
                            if((blecb.conncb[i].service_uuid_bit[m] == BLE_16BIT_UUID) && (blecb.conncb[i].charact_uuid_bit[m][n] == BLE_16BIT_UUID))
                            {
                                USART0_SendHex(5);
                                USART0_SendHex(BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE);
                                USART0_SendHex(blecb.conncb[i].service_uuid[m][1]);
                                USART0_SendHex(blecb.conncb[i].service_uuid[m][0]);
                                USART0_SendHex(blecb.conncb[i].charact_uuid[m][n][1]);
                                USART0_SendHex(blecb.conncb[i].charact_uuid[m][n][0]);
                                USART0_SendHex(evt->data.evt_gatt_characteristic_value.value.len);
                            }
							for(i = 0;i < evt->data.evt_gatt_characteristic_value.value.len;i ++)
							{
								USART0_SendHex(evt->data.evt_gatt_characteristic_value.value.data[i]);
							}
							USART0_SendStr("\r\n");
							break;
                        }
                    }
                }
            break;

            case gecko_evt_le_connection_parameters_id:
                gecko_cmd_le_connection_set_parameters(	evt->data.evt_le_connection_parameters.connection,
                                                        evt->data.evt_le_connection_parameters.interval,
                                                        evt->data.evt_le_connection_parameters.interval,
                                                        evt->data.evt_le_connection_parameters.latency,
                                                        evt->data.evt_le_connection_parameters.timeout);
				
				if(blecb.conncb[blecb.connindex].discoverstate == SCANNING)
					blecb.conncb[blecb.connindex].discoverstate = WILL_FIND_SERVICE;
				else if(blecb.conncb[blecb.connindex].discoverstate == WILL_FIND_SERVICE)
				{
					gecko_cmd_gatt_discover_primary_services(evt->data.evt_le_connection_opened.connection);
					blecb.conncb[blecb.connindex].discoverstate = FIND_SERVICE;
				}
                //sendlen = evt->data.evt_le_connection_parameters.txsize;
            break;
			
			case gecko_evt_gatt_mtu_exchanged_id:
				gecko_cmd_gatt_discover_primary_services(evt->data.evt_gatt_mtu_exchanged.connection);
			break;

            case gecko_evt_gatt_procedure_completed_id:
                if(blecb.conncb[blecb.connindex].connectedid != evt->data.evt_gatt_procedure_completed.connection)
                {
                    gecko_cmd_endpoint_close(evt->data.evt_gatt_procedure_completed.connection);
                    break;
                }
find_service:
                if(blecb.conncb[blecb.connindex].discoverstate == FIND_SERVICE)
                {
                    USART0_SendHex(RETURN_MSG_MASTER_SERVE);
                    if(blecb.conncb[blecb.connindex].service_handle[blecb.conncb[blecb.connindex].discoverserviceindex] > 0)
                    {
                        USART0_SendStr(":Service found, search for characteristic\r\n");
                        gecko_cmd_gatt_discover_characteristics(evt->data.evt_gatt_procedure_completed.connection,
                                                                blecb.conncb[blecb.connindex].service_handle[blecb.conncb[blecb.connindex].discoverserviceindex]);
                        blecb.conncb[blecb.connindex].discoverstate = FIND_CHAR;
                    }
                    else
                    {
                        USART0_SendHex(blecb.conncb[blecb.connindex].discoverserviceindex);
                        USART0_SendStr(":Service not found?!\r\n");
                        gecko_cmd_endpoint_close(evt->data.evt_gatt_procedure_completed.connection);
                    }
                    break;
                }
                if(blecb.conncb[blecb.connindex].discoverstate == FIND_CHAR)
                {
                    for(i = 0;i < MAX_EVERY_SERVICE_CHARACT_UUID_COUNTS;i ++)
                    {
                        if(blecb.conncb[blecb.connindex].charact_uuid[blecb.conncb[blecb.connindex].discoverserviceindex][i] == 0)
                            break;
                        USART0_SendHex(RETURN_MSG_MASTER_NOTIFY);
                        if(blecb.conncb[blecb.connindex].charact_handle[blecb.conncb[blecb.connindex].discoverserviceindex][i] > 0 )
                        {
                            if(blecb.conncb[blecb.connindex].charact_properties[blecb.conncb[blecb.connindex].discoverserviceindex][i] & ePROPERTIES_Notify)
                            {
                                USART0_SendStr(":Char found, turn on notifications\r\n");
                                gecko_cmd_gatt_set_characteristic_notification(evt->data.evt_gatt_procedure_completed.connection,
                                    blecb.conncb[blecb.connindex].charact_handle[blecb.conncb[blecb.connindex].discoverserviceindex][i],gatt_notification);
                            }
                            if(blecb.conncb[blecb.connindex].charact_properties[blecb.conncb[blecb.connindex].discoverserviceindex][i] & ePROPERTIES_Read)
                            {
								USART0_SendStr(":Read char timer start\r\n");
                                gecko_cmd_hardware_set_soft_timer(32768,TIMER_RERAD_CHARACT,0);
                            }
                            blecb.conncb[blecb.connindex].discoverstate = ENABLE_NOTIF;
                        }
                        else
                        {
                            USART0_SendStr(":Char not found?!\r\n");
                            gecko_cmd_endpoint_close(evt->data.evt_gatt_procedure_completed.connection);
                        }
                    }
                    break;
                }
                if(blecb.conncb[blecb.connindex].discoverstate == ENABLE_NOTIF)
                {
                    blecb.conncb[blecb.connindex].discoverserviceindex ++;
                    blecb.conncb[blecb.connindex].discoverstate = FIND_SERVICE;

                    if(blecb.conncb[blecb.connindex].discoverserviceindex >= MAX_SERVICE_UUID_COUNTS)
                    {
                        blecb.conncb[blecb.connindex].discoverstate = DATA_MODE;
                        if((blecb.connindex + 1) >= MAX_CONNECTIONS)
                        {
                            gecko_cmd_le_gap_end_procedure();
                            USART0_SendHex(RETURN_MSG_ATCMD_SCANSTOP);
                            USART0_SendStr(":SCAN STOP\r\n");
							USART0_SendHex(RETURN_MSG_MASTER_DATA);
							USART0_SendStr(":DATA\r\n");
                            break;
                        }
                        blecb.connindex = findOneEmptyIndex(blecb);
						if(blecb.connindex == 0xff)
							break;
                        blecb.conncb[blecb.connindex].discoverserviceindex = 0;
                        blecb.conncb[blecb.connindex].discoverstate = SCANNING;
                        gecko_cmd_le_gap_discover(le_gap_discover_observation);               
                        USART0_SendHex(RETURN_MSG_ATCMD_SCANSTART);
                        USART0_SendStr(":SCAN START\r\n");
                    }
                    else
                    {
                        goto find_service;
                    }
                    break;
                }
            break;

            case gecko_evt_gatt_server_user_write_request_id:

            break;

            default:
            break;
        }
    }
}

bool USART0_DMA_Callback(unsigned int channel,unsigned int sequenceNo,void *userParam)
{
    INT_Disable();

    int i = 0,dmarxlen = 0;
    if(ECODE_EMDRV_DMADRV_OK == DMADRV_TransferRemainingCount(blecb.usart0dmachannel,&dmarxlen))
    {
        dmarxlen = USART0_DMA_RX_FIFO_LEN - dmarxlen;
    }
    else
    {
        dmarxlen = 0;
    }
    if((dmarxlen >= 4) && cmpi2Array(blecb.usart0dmarxfifo,ATHEADER,strlen(ATHEADER)) && 
        ((blecb.usart0dmarxfifo[dmarxlen - 1] == '\r') || (blecb.usart0dmarxfifo[dmarxlen - 1] == '\n')))// is at cmd
    {
        cpy2Array(blecb.atcmdbuff,blecb.usart0dmarxfifo,((dmarxlen > ATCMDBUFFLEN) ? ATCMDBUFFLEN : dmarxlen)); 
        gecko_external_signal(EXSG_RECV_USR_AT_CMD);
    }
    //else if((blecb.appsta == START) && (blecb.workstyle == SERVE || blecb.workstyle == ADVER_SERVE) && (conConnectionId != 0xFF) && (charsta == ENABLE))
    //{
    //    gecko_cmd_gatt_server_send_characteristic_notification(conConnectionId, gattdb_spp_data, (dmarxlen > MTU_LEN ? MTU_LEN : dmarxlen), usart0dmarxfifo);
    //}
    //else if((appsta == START) && (workstyle == CLIENT) && (conConnectionId != 0xFF) && (main_state == DATA_MODE))
    //{
    //    gecko_cmd_gatt_write_characteristic_value(conConnectionId,char_handle,(dmarxlen > MTU_LEN ? MTU_LEN : dmarxlen), usart0dmarxfifo);
    //}
    else
    {
        USART0_SendHex(RETURN_MSG_DATAERROR);
        USART0_SendStr(":DATAERROR\r\n");
    }

    DMADRV_PeripheralMemory(blecb.usart0dmachannel,
            dmadrvPeripheralSignal_USART0_RXDATAV,
            (void *)blecb.usart0dmarxfifo,
            (void *)&USART0->RXDATA,
            true,
            USART0_DMA_RX_FIFO_LEN,
            dmadrvDataSize1,
            USART0_DMA_Callback,
            NULL);

    INT_Enable();

    return true;
}

void USART0_RX_IRQHandler(void)
{
    if(USART_IntGetEnabled(USART0) & USART_IF_TCMP1)
    {
        DMADRV_StopTransfer(blecb.usart0dmachannel);
        USART_IntClear(USART0,USART_IF_TCMP1);
        gecko_external_signal(EXSG_USART0_RX_DMA_TIMEOUT);
    }
}

void GPIO_EVEN_IRQHandler(void)
{
    uint32_t iflags;
    iflags = GPIO_IntGetEnabled() & 0x00005555;
    GPIO_IntClear(iflags);
    if(iflags & (0x00000001 << 2))
    {
        //wake up ,go to EM1/EM0
#if (EFR32BG1_BRD4100A)
        GPIO_PinOutToggle(gpioPortF, 4);
#endif
        if(sleepEM2 == SLEEP_LowestEnergyModeGet())
        {
            gecko_external_signal(EXSG_WAKEUP);
        }
    }
}

void USART0_DMA_Init(void) 
{
    DMADRV_DeInit();
    DMADRV_Init();
    DMADRV_AllocateChannel( (unsigned int *)&blecb.usart0dmachannel, NULL);
    DMADRV_PeripheralMemory(blecb.usart0dmachannel,
            dmadrvPeripheralSignal_USART0_RXDATAV,
            (void *)blecb.usart0dmarxfifo,
            (void *)&USART0->RXDATA,
            true,
            USART0_DMA_RX_FIFO_LEN,
            dmadrvDataSize1,
            USART0_DMA_Callback,
            NULL);
}

void USART0_SendStr(char *str)
{
    while(*str)
    {
        USART_Tx(USART0, (char)*str++);
    }
}

void USART0_SendBuffer(uint8_t data[],uint8_t len)
{
    for(uint8_t i = 0;i < len;i ++)
    {
        USART_Tx(USART0,data[i]);
    }
}

void USART0_SendHex(uint8_t d)
{
    uint8_t nibble = d/16;
    if(nibble < 0x0a)
        nibble += 0x30;
    else
        nibble += 0x37;
    USART_Tx(USART0, nibble);

    nibble = d & 0x0f;
    if(nibble < 0x0a)
        nibble += 0x30;
    else
        nibble += 0x37;
    USART_Tx(USART0, nibble);
}

void USART0_SendInt32(uint32_t d)
{
    uint32_t data = d;
    char i = 1,j = 1,buf[12] = {0}; 
    while(data = data / 10)
    {
        i ++;
    }
    j = i;
    data = d;
    while(data || j)
    {
        buf[-- j] = data % 10 + '0'; 
        data = data / 10;
    }
    USART0_SendBuffer(buf,i);
}

void print_mac(bd_addr addr)
{
    USART0_SendHex(addr.addr[5]);
    USART0_SendStr(":");
    USART0_SendHex(addr.addr[4]);
    USART0_SendStr(":");
    USART0_SendHex(addr.addr[3]);
    USART0_SendStr(":");
    USART0_SendHex(addr.addr[2]);
    USART0_SendStr(":");
    USART0_SendHex(addr.addr[1]);
    USART0_SendStr(":");
    USART0_SendHex(addr.addr[0]);
}

uint8_t replyATCMD(char *atcmd)
{
    uint8_t i = 0,len = 0; 
    while((len < 255) && ((atcmd[len] != '\r') && (atcmd[len] != '\n')))
    {
        len ++;
    }
    if(len >= ATCMDBUFFLEN)
    {
        i = ATCMD_ERROR;
    }
    else
    {
        i = getATCMD(atcmd,len);
    }
    USART0_SendHex(i); USART0_SendStr(":"); 
    switch(i)
    {
        case ATCMD_HEADER:
        case ATCMD_PLUS:
            blecb.appsta = START;
            USART0_SendStr(atcmdass[i].atcmdres);
        break;
        case ATCMD_DFU:
            gecko_cmd_dfu_reset((atcmd[7] - '0') & 0x03);
        break;
        case ATCMD_QUERY_MAC:
            USART0_SendStr(atcmdass[i].atcmdres);
            print_mac(blecb.local_mac);
            USART0_SendStr("\r\n");
        break;
        case ATCMD_QUERY_VERSION:
            USART0_SendStr(atcmdass[i].atcmdres);
        break;
        case ATCMD_GOTO_SLEEP:
            if(sleepEM1 == SLEEP_LowestEnergyModeGet())
            {
                SLEEP_SleepBlockEnd(sleepEM2); //END EM1
                SLEEP_SleepBlockBegin(sleepEM3);//GO EM2
                USART0_SendStr(atcmdass[i].atcmdres);
            }
        break;
        case ATCMD_SET_NAME:
            gecko_cmd_gatt_server_write_attribute_value(gattdb_device_name,0,(len - 8),&atcmd[8]);
            //gecko_cmd_gatt_server_send_characteristic_notification(conConnectionId,gattdb_device_name,(len - 8),&atcmd[8]);
            gecko_cmd_le_gap_set_mode(le_gap_non_discoverable,le_gap_non_connectable);
            gecko_cmd_le_gap_set_mode(le_gap_general_discoverable,le_gap_undirected_connectable);
            USART0_SendStr(atcmdass[i].atcmdres);
            USART0_SendBuffer(&atcmd[8],(len - 8));
            USART0_SendStr("\r\n");
            break;
        case ATCMD_QUREY_CONSTA:
            USART0_SendStr(atcmdass[i].atcmdres);
            //USART0_SendHex(conConnectionId);
            USART0_SendStr("\r\n");
        break;
        case ATCMD_SET_PIN:
            blecb.pin = mintl(atcmd,7,len,10);
            gecko_cmd_sm_set_passkey(blecb.pin);
            USART0_SendStr(atcmdass[i].atcmdres);
            USART0_SendBuffer(&atcmd[7],(len - 7));
            USART0_SendStr("\r\n");
        break;
        case ATCMD_DISCON:
            //if((conConnectionId != 0xFF))
            //{
            //    discontime = mintl(atcmd,10,len,10) & 0x7FFFFFFF;
            //    if(discontime)
            //    {
            //        gecko_cmd_hardware_set_soft_timer(33 * discontime,TIMER_CON_START,1);
            //        discontime |= 0x80000000;
            //    }
            //    gecko_cmd_endpoint_close(conConnectionId);

            //    USART0_SendStr(atcmdass[i].atcmdres);
            //    USART0_SendBuffer(&atcmd[10],(len - 10));
            //    USART0_SendStr("\r\n");
            //}
            //else
            //{
            //    USART0_SendStr("error 0x01:no connecting\r\n");
            //}
        break;
        case ATCMD_QUERY_ALL:
            USART0_SendStr(atcmdass[i].atcmdres);

            USART0_SendStr("LOCAL MAC:");
            print_mac(blecb.local_mac);
            USART0_SendStr("\r\n");

            USART0_SendStr("NAME:");
            USART0_SendBuffer(gecko_cmd_gatt_server_read_attribute_value(gattdb_device_name,0)->value.data,gecko_cmd_gatt_server_read_attribute_value(gattdb_device_name,0)->value.len);
            USART0_SendStr("\r\n");

            USART0_SendStr("VERSION:");
            USART0_SendStr(atcmdass[ATCMD_QUERY_VERSION].atcmdres);
            USART0_SendStr("\r\n");

            USART0_SendStr("APPSTA:");
            USART0_SendStr((blecb.appsta == START) ? "START" : "STOP");
            USART0_SendStr("\r\n");

        break;

        default:
            USART0_SendStr(atcmdass[ATCMD_ERROR].atcmdres);
        break;
    }

    return 0;
}

void resp_evt_scan_response(struct gecko_cmd_packet* evt)
{
    static uint8_t scan_counts = 0;
    static int8_t rssi = -127;
    static bd_addr mac;

    uint8_t index = 0;
    if(blecb.connindex >= MAX_CONNECTIONS)
    {
        USART0_SendHex(RETURN_MSG_ATCMD_SCANSTOP);
        USART0_SendStr(":SCAN STOP\r\n");
        gecko_cmd_le_gap_end_procedure();
        return;
    }
    while(index < evt->data.evt_le_gap_scan_response.data.len)
    {
        uint8_t field_length = evt->data.evt_le_gap_scan_response.data.data[index];
        uint8_t field_type = evt->data.evt_le_gap_scan_response.data.data[index + 1];
        if((field_type >= BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_MORE_AVAILABLE) && (field_type <= BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE))
        {
            for(uint8_t i = 0;i < (field_length - 1);)
            {
                if(cmp2Array(&evt->data.evt_le_gap_scan_response.data.data[index + 2],
                (uint8_t *)blecb.conncb[blecb.connindex].service_uuid[blecb.conncb[blecb.connindex].mainservice],
                blecb.conncb[blecb.connindex].service_uuid_bit[blecb.conncb[blecb.connindex].mainservice] / 8))
                {
                    if(scan_counts < 128)
                    {
                        if(evt->data.evt_le_gap_scan_response.rssi > rssi)
                        {
                            cpy2Array(mac.addr,evt->data.evt_le_gap_scan_response.address.addr,6);
                            rssi = evt->data.evt_le_gap_scan_response.rssi;
                        }
                        scan_counts ++;
                        return;
                    }
                    if(!cmp2Array(mac.addr,evt->data.evt_le_gap_scan_response.address.addr,6))
                        return;

                    gecko_cmd_le_gap_end_procedure();
                    gecko_cmd_le_gap_set_conn_parameters(200,200,0,100);
                    if(gecko_cmd_le_gap_open(evt->data.evt_le_gap_scan_response.address,evt->data.evt_le_gap_scan_response.address_type)->result)
                    {
                        gecko_cmd_le_gap_discover(le_gap_discover_observation);               
                    }

                    scan_counts = 0;
                    rssi = -127;
                    return;
                }
                i += blecb.conncb[blecb.connindex].service_uuid_bit[blecb.conncb[blecb.connindex].mainservice] / 8;
            }
        }
        index = index + field_length + 1;
    }
}

size_t _write(int handle, const unsigned char * buffer, unsigned char size)
{
    /* Remove the #if #endif pair to enable the implementation */
    unsigned char nChars = 0;
    if (buffer == 0)
    {
        return 0;
    }
    for (/* Empty */; size != 0; --size)
    {
        USART_Tx(USART0,(char)buffer[nChars]);
        ++ nChars;
    }
    return nChars;
}

